
#include "../include/location.hpp"

LocationInfo::LocationInfo(/* args */) {}

LocationInfo::~LocationInfo() {}


void LocationInfo::run() {
    this->model_state_sub = nh.subscribe("/gazebo/model_states_low_freq", 
            1, &LocationInfo::model_state_callback, this);
}


void LocationInfo::model_state_callback(const gazebo_msgs::ModelStates &msg) {
    geometry_msgs::Pose smart_pose = msg.pose[1];
    geometry_msgs::Twist smart_twist = msg.twist[1];
    double center_x = smart_pose.position.x;       // 小车中心位置坐标
    double center_y = smart_pose.position.y;
    // 四元组转欧拉角
    tf::Quaternion quat;
    tf::quaternionMsgToTF(smart_pose.orientation, quat);
    double roll, pitch, yaw;
    // 这里计算的角度范围是 [-pi, pi]
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);

    Location host;
    host.x = center_x - cos(yaw) * 0.945;   // 计算后轮的坐标 x
    host.y = center_y - sin(yaw) * 0.945;   // 后轮坐标 y
    host.heading = yaw;
    host.speed = smart_twist.linear.x / cos(yaw);
    host.v_x = smart_twist.linear.x;
    host.v_y = smart_twist.linear.y;
    if (FLAG_is_first_run_model_state_callback) {
        host.kappa = host.heading / 0.945;
        host.accel = host.speed / 0.1;
        host.a_x = host.accel * cos(host.heading);
        host.a_y = host.accel * sin(host.heading);
        this->car_location  = this->car_pre_location = host;
        FLAG_is_first_run_model_state_callback = false;
    } else {
        // 上一时刻的 heading 减去当前时刻的 heading
        double d_heading = abs(this->car_pre_location.heading - yaw);
        // 上一时刻小车后轮位置与当前时刻的小车中心位置的距离
        double d_dis = sqrt(pow(car_pre_location.x - center_x, 2) + pow(car_pre_location.y - center_y, 2));
        host.kappa = d_heading / d_dis;

        host.accel = (host.speed - this->car_pre_location.speed) / 0.1;
        host.a_x = host.accel * cos(host.heading);
        host.a_y = host.accel * sin(host.heading);
        this->car_pre_location = this->car_location;
        this->car_location = host;
    }
    // locationToString();
}


void LocationInfo::locationToString() {
    std::cout << "x: " << car_location.x << " y: " << car_location.y << " heading: " 
            << car_location.heading * 180 / PI << " kappa: " << car_location.kappa << " speed: "
             << car_location.speed << " accel: " << car_location.accel << std::endl; 
}